A navigation filter for fusing DTM/correspondence updates. 



. . . Oleg Kupervasser, Vladimir Voronov 

o 

Q_J Transas group of companies 

-^ ■ Moscow, Russia 

^r^ ■ Email: olegkup@yahoo.com 

(n: 

>: 

U; 

O; 

m 

>: 
(^. 

m 

o 

1^. August 2011 

o: 



X: 



AHHOTaii;HH 

An algorithm for pose and motion estimation using corresponding features in 
images and a digital terrain map is proposed. Using a Digital Terrain (or Digital 
Elevation) Map (DTM/DEM) as a global reference enables recovering the absolute 
position and orientation of the camera. In order to do this, the DTM is used to 
formulate a constraint between corresponding features in two consecutive frames. 
The utilization of data is shown to improve the robustness and accuracy of the 
inertial navigation algorithm. Extended Kalman filter was used to combine results 
of inertial navigation algorithm and proposed vision-based navigation algorithm. 
The feasibility of this algorithms is established through numerical simulations. 



0.1 Introduction. 

Vision-based algorithms has been a major research issue during the past decades. 
Two common approaches for the navigation problem are: landmarks and ego-motion 
integration. In the landmarks approach several features are located on the image- 
plane and matched to their known 3D location. Using the 2D and 3D data the 
camera's pose can be derived. Few examples for such algorithms are [2j, [3]. Once the 
landmarks were found, the pose derivation is simple and can achieve quite accurate 
estimates. The main difficulty is the detection of the features and their correct 
matching to the landmarks set. 

In ego-motion integration approach the motion of the camera with respect to 
itself is estimated. The ego-motion can be derived from the optical-flow field, or 
from instruments such as accelerometers and gyroscopes. Once the ego-motion was 
obtained, one can integrate this motion to derive the camera's path. One of the 
factors that make this approach attractive is that no specific features need to be 
detected, unlike the previous approach. Several ego-motion estimation algorithms 
can be found in P], [5], [6], [7]. The weakness of ego- motion integration comes from 
the fact that small errors are accumulated during the integration process. Hence, 
the estimated camera's path is drifted and the pose estimation accuracy decrease 
along time. If such approach is used it would be desirable to reduce the drift by 
activating, once in a while, an additional algorithm that estimates the pose directly. 
In [8j, such navigation-system is being suggested. In that work, like in this work, the 
drift is being corrected using a Digital Terrain Map (DTM). The DTM is a discrete 
representation of the observed ground's topography. It contains the altitude over 
the sea level of the terrain for each geographical location. In [S] a patch from the 
ground was reconstructed using ' structure- from-motion' (SFM) algorithm and was 
matched to the DTM in order to derive the camera's pose. Using SFM algorithm 
which does not make any use of the information obtained from the DTM but rather 
bases its estimate on the fiow-field alone, positions their technique under the same 
critique that applies for SFM algorithms [1]. 

The algorithm presented in this work does not require an intermediate explicit 
reconstruction of the 3D world. By combining the DTM information directly with the 
images information it is claimed that the algorithm is well- conditioned and generates 
accurate estimates for reasonable scenarios and error sources. In the present work 
this claim is explored by performing an error analysis on the algorithm outlined 
above. By assuming appropriate characterization of these error sources, a closed 
form expression for the uncertainty of the pose and motion of the camera is first 
developed and then the infiuence of different factors is studied using extensive 
numerical simulations. 



0.2 Problem Definition and Notations. 

The problem can be briefiy described as follows: At any given time instance t, a 
coordinates system C{t) is fixed to a camera in such a way that the Z-axis coincides 
with the optical-axis and the origin coincides with the camera's projection center. 
At that time instance the camera is located at some geographical location p{t) and 
has a given orientation R{t) with respect to a global coordinates system W {p{t) 
is a 3D vector, R{t) is an orthonormal rotation matrix). p{t) and R{t) define the 
transformation from the camera's frame C{t) to the world's frame W, where if ^v 
and ^v are vectors in C{t) and W respectively, then ^v = R{t)^v + p{t). 

Consider now two sequential time instances ti and ^2- the transformation from 



C{ti) to C{t2) is given by the translation vector Ap{ti,t2) and the rotation matrix 
AR(ti,t2), such that "^^^^^v = AR {11,12)"^^*^^ v + Ap(ti,t2). A rough estimate of the 
camera's pose at ti and of the ego- motion between the two time instances - pE{ti) 
,RE{ti), ApE{ti,t2) and Ai?s(ti,t2) - are supphed (the subscript letter "E" denotes 
that this is an estimated quantity). 

Also supplied is the optical-flow field: {ui{tk)} (i=l...n, k=l,2). For the i'th 
feature, Ui{ti) G M^ and Ui{t2) G M^ represent its locations at the first and second 
frame respectively. 

Using the above notations, the objective of the proposed algorithm is to estimate 
the true camera's pose and ego-motion: p{ti), R{ti), Ap(ti,t2) and AR{ti,t2), using 
the optical-flow field {ui{tk)}, the DTM and the initial-guess: PE{ti), RE{ti), ApE{ti,t2) 
and ARE(ti,t2). 

0.3 The Navigation Algorithm 

The following section describes a navigation algorithm which estimate the above 
mentioned parameters. The pose and ego-motion of the camera are derived using 
a DTM and the optical-flow fleld of two consecutive frames. Unlike the landmarks 
approach no speciflc features should be detected and matched. Only the correspondence 
between the two consecutive images should be found in order to derive the optical- 
flow fleld. As was mentioned in the previous section, a rough estimate of the required 
parameters is supplied as an input. Nevertheless, since the algorithm only use this 
input as an initial guess and re-calculate the pose and ego-motion directly, no 
integration of previous errors will take place and accuracy will be preserved. 

The new approach is founded on the following observation. Since the DTM 
supplies information about the structure of the observed terrain, depth of observed 
features is being dictated by the camera's pose. Hence, given the pose and ego-motion 
of the camera, the optical-flow fleld can be uniquely determined. The objective of 
the algorithm will be flnding the pose and ego-motion which lead to an optical-flow 
fleld as close as possible to the given flow fleld. 

A single vector from the optical-flow fleld will be used to deflne a constraint 
for the camera's pose and ego-motion. Let ^'G G M'^ be a location of a ground 
feature point in the 3D world. At two different time instances ti and t2, this feature 
point is projected on the image-plane of the camera to the points u{ti) and ^(^2). 
Assuming a pinhole model for the camera, then u{ti),u{t2) G M^. Let '^g(ti)and 
^q{t2) be the homogeneous representations of these locations. As standard, one can 
think of these vectors as the vectors from the optical-center of the camera to the 
projection point on the image plane. Using an initial-guess of the pose of the camera 
at ti, the line passing through PE{ti) and ^'q{ti) can be intersected with the DTM. 
Any ray-tracing style algorithm can be used for this purpose. The location of this 
intersection is denoted as ^'Ge- The subscript letter "£"' highlights the fact that this 
ground-point is the estimated location for the feature point, that in general will be 
different from the true ground-feature location ^G. The difference between the true 
and estimated locations is due to two main sources: the error in the initial guess for 
the pose and the errors in the determination of ^Ge caused by DTM discretization 
and intrinsic errors. For a reasonable initial-guess and DTM-related errors, the two 
points ^Ge and ^G will be close enough so as to allow the linearization of the DTM 
around ^Ge- Denoting by A^ the normal of the plane tangent to the DTM at the 
point ^Ge, one can write: 



The true ground feature ^G can be described using true pose parameters: 

-G = R{h) ■ ^g(ti) • A + p{h) 



(2) 



Here, A denotes the depth of the feature point (i.e. the distance of the point to the 
image plane projected on the optical-axis). Replacing (E]) in ([1]): 



N^{\ ■ Rih) ■ 'qih 
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(3) 



From this expression, the depth of the true feature can be computed using the 
estimated feature location: 



A 



N^'^Ge - N^piti 



N^Rihrqih) 
By plugging (^ back into ([2]) one gets: 



(4) 



'G = Rihrqih) 



N^Rihrqih) 



P{ti 



(5) 



In order to simplify notations, R{ti) will be replaced by Ri and likewise for p{ti) and 
q{ti) i = 1,2. AR{ti,t2) and Ap{ti,t2) will be replaced by Ru and pu respectively. 
The superscript describing the coordinate frame in which the vector is given will 
also be omitted, except for the cases were special attention needs to be drawn to the 
frames. Normally, pi2 and g's are in camera's frame while the rest of the vectors are 
given in the world's frame. Using the simplified notations, (^ can be rewritten as: 
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In order to obtain simpler expressions, define the following projection operator: 



(6) 



V{u,s) 



I 



us 



(7) 



This operator projects a vector onto the subspace normal to s, along the direction 
of u. As an illustration, it is easy to verify that s^ ■ V{u, s)v = and V{u, s)u = 0. 
By adding and subtracting Ge to ([6]), and after reordering: 



G = Ge + 
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N^RiQi 
Using the projection operator, (|8]) becomes: 

G = GE + V{R,qi,N){p,-GE 



G, 



(8) 



(9) 



The above expression has a clear geometric interpretation (see FigH]). The vector 
from Ge to pi is being projected onto the tangent plane. The projection is along 
the direction Riqi, which is the direction of the ray from the camera's optical-center 
(pi), passing through the image feature. 

Our next step will be transferring G from the global coordinates frame- W into 
the first camera's frame Gi and then to the second camera's frame G2. Since piand 
/?idescribe the transformation from Gi into W, we will use the inverse transformation: 



C2 



G=Pi2 + Ri2{RUG-Pi)) 



(10) 




'P(R,q,,w)(p,-t3£) 



G 



Phc. 1: Geometrical description of expression (jH]) using the projection operator ([7]) 

Assigning (Q into ( TTOj) gives: 

^^G = pu + Ri2C{Ge-Pi) (11) 

C in the above expression represents: 

C - ^^^^ (12) 

One can think of C as an operator with inverse characteristic to V: it projects vectors 
on the ray continuing RiQi along the plane orthogonal to A^. 

g2 is the projection of the true ground-feature G. Thus, the vectors g2 and '^^G 
should coincide. This observation can be expressed mathematically by projecting 
^'^G on the ray continuation of q2'- 

C2G = -Sl^. (p- . ^2g\ (13) 

1^21 Km J 

In expression (IT5|) . 5'2'/|5'2| ■ ^'^G is the magnitude of ^^(^'s projection on g2- By 
reorganizing (I13p and using the projection operator, we obtain: 



\ _ g2 • ql 

T 
% ■?2, 



■^^G = V{q2,q2)-'''G = (14) 



^'^G is being projected on the orthogonal complement of g2- Since ^'"^G and g2 should 
coincide, this projection should yield the zero-vector. Plugging (ITT]) into (IT1|) yields 
our final constraint: 

Viq2, q2) [Pi2 + R12C (Ge - Pi)] = (15) 

This constraint involves the position, orientation and the ego-motion defining the 
two frames of the camera. Although it involves 3D vectors, it is clear that its rank 
can not exceed two due to the usage of V which projects M.^ on a two-dimensional 
subspace. 

Such constraint can be established for each vector in the optical-flow field, until 
a non-singular system is obtained. Since twelve parameters need to be estimated (six 
for pose and six for the ego- motion), at least six optical-flow vectors are required 
for the system solution. But it is correct conclusion for nonlinear problem. If we 
use Gauss- Newton iterations method and so make linearization of our problem near 
approximate solution. The found matrix will be always singular for six points (with 



zero determinant) as numerical simulations demonstrate. So it is necessary to use at 
least seven points to obtain nonsingular linear approximation. Usually, more vectors 
will be used in order to define an over-determined system, which will lead to more 
robust solution. The reader attention is drawn to the fact that a non-linear constraint 
was obtained. Thus, an iterative scheme will be used in order to solve this system. A 
robust algorithm which uses Gauss-Newton iterations and M-estimator is described 
in [S].We begin to use Levenberg-Marquardt method if Gauss-Newton method after 
several iterations stopped to converge. This two algorithms are realized in lsqnonlin() 
Matlab function. The applicability, accuracy and robustness of the algorithm was 
verified though simulations and lab-experiments. 

It is more convenient to use more robust for iterations equivalent to flT5]) equation: 

V{q2, q2) [Pi2 + Ri2C, {Ge, - pi)] /r^G| = (16) 

Using of this normalized form of equations avoids to get incorrect trivial solution 
when two positions are in a single point on the ground. 

0.4 Vision-based navigation algorithm corrections for inertial 
navigation by help of Kalman filter. 

Vision-based navigation algorithms has been a major research issue during the past 
decades. Algorithm used in this paper is based on foundations of multiple-view 
geometry and a land map. By help of this method we get position and orientation 
of a observer camera. On the other hand we obtain the same data from inertial 
navigation methods. To adjust these two results Kalman filter is used. We employ 
in this paper extended Kalman filter for nonlinear equations [12 j. 

For inertial navigation computations was used Inertial Navigation System Toolbox 
for Matlab [13]. 

Input of Kalman filter consists of two part. The first one is variables X for 
equations of motion. In our case it is inertial navigation equations. Vector X consists 
of fifteen components: [8x Sy Sz SV^ SVy SV^ S(j) 66 Sip a^,. ay a^ b^ by b^]. Coordinates 
Sx 6y 6z are defined by difference between real position of the camera and position 
gotten from inertial navigation calculus. Variables SV^SVySYz are defined by difference 
between real velocity of the camera and velocity gotten from inertial navigation 
calculus. Variable 6(j) 69 6ip are defined as Euler angles of matrix Dj. * Dj where Dr 
is matrix defined by real Euler angles of camera with respect to Local Level Frame 
(L- Frame) and Dc is matrix defined by Euler angles of camera with respect to Local 
Level Frame (L-Frame) gotten by inertial navigation computation. It is necessary to 
pay attention that found Euler angles 6(f) 69 6ip ARE NOT equivalent to difference 
between real Euler angles and Euler angles gotten from inertial navigation calculus. 
For small values of 6(j) 69 6ip perturbations to these angles can be added linearly 
and so these angles can be used in Kalman filter for small errors. Such choose of 
angles is made because formulas describing their evolution are much simpler than 
formulas describing evolution of Euler angles differences. Variables a^aya^ are defined 
by vector of Accel bias in inertial navigation measurements. Variables b^ by b^ are 
defined by vector of Gyro bias in inertial navigation measurements. 

The second input of Kalman filter is Z-result of measurements by vision-based 
navigation algorithms. Vector Z consists of six components [6xm6ym6zm6(j)m69m6ipm]CooTdmaies 
6xm6ym6zm are difference between camera position measured by vision-based navigation 
algorithm and position gotten from inertial navigation calculus. Variable 69jn6il'm are 
defined as Euler angles of matrix Dm * Dj where D^ is matrix defined by Euler 



angles of camera with respect to Local Level Frame (L-Frame) measured by vision- 
based navigation algorithm and D^ is matrix defined by Euler angles of camera with 
respect to Local Level Frame (L-Frame) gotten by inertial navigation computation. 
Let variable k to be number of step for time discretization used in Kalman filter. 

We assume that errors between values gotten by inertial navigation computation 
and real values are linearly depend on noise. Corespondent process noise covariance 
matrix is denoted by Q^. Diagonal elements of Q^ correspondent to velocity are 
defined by Accel noise and proportional to dt^: Qy ~ dt"^, where dt is time interval 
between t^ and tfc-i^ dt = tk — tfc-i- Diagonal elements of Qk correspondent to Euler 
angles are defined by Gyro noise and proportional to dt: Qa ~ dt. 

We assume that errors between values gotten by vision-based navigation algorithm 
and real values are linearly depend on noise. Corespondent measurement noise 
covariance matrix is denoted by R^. Error analysis giving this matrix is described 
in ^. 

Kalman filter equations describe evolution of a posteriori state estimation Xk 
described above and aposteriori error covariation covariance matrix Pk for variables 

To write Kalman filter equations we must define two 15x15 matrices jet:Hk and 
Ak- Matrix Hk is measurement Jacobian describing connection between predicted 
measurement Hk*Xk and actual measurement Zk defined above. Diagonal elements 
Hk{l,l), Hk{2,2), Hk{3,3) describing coordinate and elements iffc(4, 7), Hk{5,8), 
Hk{Q,9) describing angles are equal to one. The rest of the elements are equal to 
zero. 

Ak is Jacobian matrix describing evolution of vector Xk- The exact expression 
for this matrix is very difficult so we use approximate formula for Ak neglecting 
by Coriolis effects, Earth rotation and so on. Let (p 6 ip he the Euler angles in L- 
Frame, dV is deltaV vector gotten from inertial navigation measurements, /„ec is 
acceleration vector in L-frame, DCM|^_^q_j is direction cosine matrix (from body- 
frame to L-frame). 

The formulas defining Ak are follow: 

cos('?/') sin(^) 
^DCM= I -sin(^) cos(V') I (17) 

1 

cos{9) -sin{9) 
Qdcm = I 1 I (18) 

sin(^) cos(e) 

10 

^DCM = I cos(</)) sin(0) I (19) 

— sin(0) cos(0) 

DCM^_^^_l = ^dcmQdcm^dcm (20) 

/.ec = DCM^_^^_^^ (21) 



Phi{l :3,4: 6) = 1 (22) 




^ Jvec \^ ) Jvec \^ ) 

Phi{A : 6, 7 : 9) = I /,ec(3) -/.ec(l) | (23) 

-/.ec(2) /.ec(l) 

Phi{7 : 9, 10 : 12) = -DCM^_^q_y (24) 

Phi{A : 6, 13 : 15) = -DCM^_^^^ (25) 

The rest of elements for matrix Phi are equal to zero. 

Ak = I + Phi dt (26) 

Kalman filter time update equations are follow: 

X^ = [0 00000000 a^k-1 Sfc-i "^fc-i ^^k-i Kk-i ^zk-i] (27) 

P,- = A^P^.^Al + Q,.i (28) 

Kalman filter update equations project the state and covariance estimates from 
the previous time step fc — 1 to the current time step k. 
Kalman filter measurement update equations are follow: 

K, = P^Hl{H,Pi:Hl + R,)-^ (29) 

Xk = X, + Kk{Zk - H,X,) (30) 

Pk = il- KkHk)P,{I - KkHkY + KuRkKl (31) 

Kalman filter measurement update equations correct the state and covariance 
estimates with measurement Z^. 

The found vector X^. is used to update coordinates, velocities, Euler angles, Accel 
and Gyro biases for inertial navigation calculations on the next step. 

Numerical simulations were realized to examine effectiveness of Kalman filter to 
combine these two navigation algorithms. On Fig. [2] we can see that corrected path 
for coordinate error much smaller than inertial navigation coordinate error without 
Kalman filter. Improved results by help Kalman filter are gotten also for velocity in 
spite of the fact that this velocity was not measured by help vision-based navigation 
algorithm Fig. |3l 

0.5 Conclusions 

An algorithm for pose and motion estimation using corresponding features in images 
and a DTM was presented with using Kalman filter. The DTM served as a global 
reference and its data was used for recovering the absolute position and orientation 
of the camera. In numerical simulations position and velocity estimates were found 
to be sufficiently accurate in order to bound the accumulated errors and to prevent 
trajectory drifts. 
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Phc. 2: Position errors ((a) for x coordinate (b) for y coordinate (c) for z coordinate) 
of the drift path are marked with a red hne, and errors of the corrected path are 
marked with a blue hne. 
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Phc. 3: (a) Velocity errors of the drift path (x y z components), and (b) Velocity 
errors of the corrected path (x y z components). 
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AHH0Taii;H5I 

Hpe^jiojKeH ajiropHTM j^jis HaxojK^eHHH no3Hn;HH, opneHTai^HH h on;eHKH ^BHJKe- 
HHs, HcnojibsyiomHH cooTBexcTByioiuHe TOHKH B H3o6pa>KeHH5ix H n;H4)poByio Kapxy 
jiaHflma4)Ta. HcnojibsoBaHne LI,H(|)poBOH Kapxbi JIaHflma(|)Ta (LI,KJ1) xax rjio6ajib- 
HOH cnpaBOHHOH HH4)opMan;HH nosBojiHex BoccxaHOBjieHHe a6cojiioTHOH no3Hij,HH h 
opHeHTan;HH KaMepbi. HtoGbi c^^ejiaxb sto, LI,KJI HcnojibsyexcH, htoGbi ccjDopMyjiH- 
poBaTb ;];onojiHHTejibHbie orpaHHHeHHH ueyKpy cooTBeTCTByiomHMH TOHKaMH b ^Byx 
nocjie;];oBaTejibHt.ix K&pp&x. HcnojibsOBaHne sthx ji&HHhix nosBOjiaex yjiyHniHTb na- 
;];e>KHocTb H TOHHocTb HHepn;HajibHoro HaBHran;HOHHoro ajiropHXMa. PacniHpeHHbiH 
(Jdhjibtp KajiMana ncnojibsoBajicH, HTo6t.i oGt-e^^nHHTb pesyjibTaxbi HHepn;HajibHoro 
HaBHran,HOHHoro ajiropHXMa h naBHrai^HOHHoro ajiropaxMa, ocHOBaHHoro na kom- 
nbioTepHOM speHHH. BbinojiHHMocTb sToro ajiropHTMa npofleMOHCTpHpoBana nyxeM 

HHCJieHHOrO MO;];ejIHpOBaHH3. 



0.6 BBejieRiie. 

OcHOBaHHbie Ha cncxeMe xexHHHecKoro spenna ajiropHTMbi Gbijih rjiaBHOii Hccjie- 
flOBaTejibCKoit npoGjieMoti b xeneHHe npomjibix flecaTHjiexHH. /I,Ba e^HHtix noflxo- 
^a flji3 HaBHrai];HOHHOH npoGjieMbi cymecTByioT: HaaeMHbie opHeHTHpbi h HHTerpa- 
ii;h3 co6cTBeHHoro flBHJKeHHa. B no^xo^e, ocHOBaHHOM na naaeMHtix opneHTHpax, 
HecKOJiBKo xapaKTepHbix o6i.eKTOB Haxo^^axcH na hjiockocth HsoGpaaceHHa h cBepa- 
lOTca no Hx HSBecxHOMy xpexMepnoMy MecTonojioaceHHio. Hcnojibsya 2-L.ie h xpex- 
MepHbie fl^aHHhie, Moryx Gbixb nojiyneHbi nojioxceHHe h opHeHxan;Ha KaMept.1. Hcmho- 
ro npHMepoB j^jis xaKHx ajiropnxMOB - [2], [3]. KaK xojibko HaseMHbie opHeHXHpt.i 
GbiJiH Hait^eHbi, naxoac^eHHe nojiojKeHHsi h opneHxai^HH KaMepti npocxo h Moacex 
GbixL. ;i,ocxHrHyxo BecbMa xohho. OcHOBHaa xpy;];Hocxb - oGnapyaceHHe HaseMHbix 
opneHXHpoB H HaxojK^eHHe hx npaBHjitHoro cooxBexcxBHa k o6x.eKxaM h3 HSBecx- 
Horo HaGopa. 

B Mexo^e HHxerpan;HH co6cxBeHHoro ^BHJKeHHa, ^BHaceHHe KaMept.1 oxHocHxejit- 
Ho ceGa caMofl on;eHHBaexcH. ^BHJKeHHe 3ro MOJKex 6t>ixt. nojiyneHo h3 hojih ohxh- 
HecKoro noxoKa, hjih h3 npnGopoB, xaKHX KaK aKcejiepoMexpti h rnpocKonbi. KaK 
xojiBKo coGcxBeHHoe ;],BH>KeHHe Gbijio nojiyneHo, mojkho HHxerpnpoBaxB sxo ^Bnace- 
HHe H HaitxH nyxb KaMept.1. O^hh hs K03c|)(J)Hn;HeHxoB, Koxoptie ^ejiaiox sxox no^xo^ 
npHBjieKaxejiBHbiM, - xo, hxo HHKaKHx HaaeMKbix opneHXHpoB ne nyacHo naxo^^nxb, 
B oxjiHHHe ox npe^biflymero no^xo^a. HecKOjibKO ajiropnxMOB oti;eHKH co6cxBeHHoro 
fl^BnyKenns Moryx 6t.ixt. Hafl^^ent.! b [¥j, [5j, [6j, [7j. 

He^ocxaxoK Mexo^a HHxerpan,HH co6cxBeHHoro ^BHJKeHHa npoHcxo^HX h3 xoro 
o6cxo3xejit.cxBa, hxo MajientKHe omnGKH cyMMHpyioxca bo BpeM3 npoti;ecca nnxe- 
rpaii;HH. Cjie^oBaxejitno, omnGKa npe^nojiaraeMoro nojiojKenHsi h opHeHxan;HH Ka- 
Mepbi HaKanjiHBaexcH h npoHcxo^nx yMeHtmeHHe xohhocxh on;eHKH no3t.i c xene- 
HHeM BpeMeHH. EcjiH xaKofl no^xo^ HcnojiB3yexca, 6t.ijto 6t.i acejiaxejiBHO yMent- 
niHxt. HaKonjieHHe ouirGkr, aKXHBH3Hpy3, BpeMa ox BpeMenn, flonojtHHxejibHt.iH 
ajiropHXM, KoxoptiH oti;eHHBaex no3y nenocpeflcxBeHHo. B |8] npefljiaraexca xaKaa 
HaBHrati;HOHHa3 cncxeMa. B sxott paGoxe, xaKace KaK h b nacxoHmeii paGoxe, naKon- 
jieHHe omnGKH HcnpaBjiaexca, Hcnojib3y5i IJ,H4)poByio Kapxy JIaH;];ma(|)xa (LI,KJI). 
I^KJI - flHCKpexHoe npe^cxaBjienHe xonorpa(|)HH HaGjnoflaeMoit MecxHocxn. Oho co- 
fl^epyKRT Bbicoxy na^ ypoBHCM Mopa jiaH;];ma4)xa fljia Ka>K;];oro reorpa(|)HHecKoro 
MecxonojiojKeHHH. B j8] KycoK MecxHocxH 6t.ijt BoccxaHOBjien, Hcnojib3ya 'cxpyKxy- 
py H3 ;];BH>KeHHH' (CH/I,) ajiropHXM, h corjiacoBan c IJ^KJI, Hxo6t.i nojiynnxt. no3y 
KaMept.1. Hcnojib30BaHHe ajiropnxMa CH^, Koxopt.ifi ne Hcnojit.3yex HH(|)opMati;HK), 
nojiyneHHyio h3 LI,KJI, a CKopee ocnoBbiBaex cboio ou;eHKy na o^hom xojibko nojie 
onxHHecKoro noxoKa, MOJKex 6t.ixt> noflBepjKeno xoMy >Ke KpnxHHecKOMy anajiHsy, 
Hxo H caM ajiropHXM CHJX ^■ 

AjiropHXM, Hpe^cxaBjieHHtiH b sxoh pa6oxe, ne xpeGyex npoMeacyxoHHOH sbhoh 
peKOHCxpyKH,HH xpexMepHoro MHpa. KoMGnnnpya HH(|)opMan;Hio h3 LI,KM nenocpe^- 
cxBeHHo c HH(|)opMan;HeH 113 ii3o6pa>KeHiiH, ajiropiixM xopomo o6ycjioBjieH h rene- 
pnpyex xoHHbie oti;eHKH ;i,jia pa3yMHt.ix cn;eHapHeB h pa3yMHt.ix ohihGok. B jxanHOR 
pa6oxe axo xpeGoBaniie Hccjie^yexca, Bbinojinaa aHajiH3 ouihGok na ajiropuxMa, bbi- 
^ejieHHOM Bbime. Onpej^ejiss cooxBexcxByiotu;He xapaKxepncxiiKii sxnx iicxohhhkob 
omnGoK, nojiynaeM Bt.ipa>KeHHe jiJis Heonpe;];ejieHHOcxH no3t.i h ^^BiixceHUH KaMe- 
pt.1 H 3axeM H3yHaeM BjiHiiHHe pa3JiHHHt.ix K03(|)4)Hn;HeHxoB, HcnojiB3ya oGmnpHoe 
HHCjioBoe Mo^ejiHpoBaHHe. 



0.7 Onpe^ejieHHe h onHcaHHe npo6jieMbi. 

HpoGjieMa Moxcex 6t>iTb KpaxKO onncana cjie;];yioin;HM o6pa30M: B jiio6oh momcht 
BpeMCHH t, cHCTCMa Koop^HHaT C{t) ycTaHOBjieHa Ha KaMepy xaKHM cnoco6oM, hto 
Z-och coBna^aex c onTHHecKoil ocbio, h nanajio Koop^^HHax coBna^aex c i^enxpoM 
npoeKTHpoBaHHH KaMept.1. B stot momcht BpeMenn Kanepa pacnojiojKena b neKo- 
TopoM reorpa(|)HHecKOM MecxonojiojKeHHH p[t) h HMeex ^aHHyio opHeHTan;Hio R{t) 

OTHOCHXejIBHO rjIo6ajIbHOH KOOp^^HHaTHOH CHCXeMBI W {p{t) - XpeXMepHblH BCKTOp, 

R{t) - opTOHopMajiBHaa MaTpHii,a BpameHHJi). p{t) h R{t) onpe^^ejiHioT iipeo6pa3o- 
BaHHe H3 KoopflHHaxHOH CHCxeMbi KaMept.1 C{t) B rjio6ajibHyio Koop^HHaxnyio ch- 
cxeMy W, rflfi, ecjiH ^'v h ^'v hbjisioxch BexxopaMH b C{t) a W cooxBexcxBCHHo, xo 
'^v = R{tfv + p{t). 

PaccMoxpHM xenepb ^Ba nocjie;];oBaxejibHL.ix MOMenxa BpeMenn ti h t2: npeo6pa- 
30BaHHe H3 C(ti) B C{t2) ^aHo BeKxopoM c^BHra Ap{ti,t2) H MaxpHn,eH Bpain,eHHH 
AR{ti,t2) xaKHM o6pa30M, mo '^''2)^ = /\R{ti,t2)^^^^''v + Ap{ti,t2). JXjm rpyGoit 
oii;eHKH no3t.i KaMepti b ti h coGcxBeHHoe ^BHaceHHe KaMepbi ueyKpy ^Byna momgh- 
xaMH BpeMeHH - pE(ti), RE(ti), ApE(ti,t2) H ARE(ti,t2) Hcnojib3yioxcH. (Chmboji 
"i5"^jiH HHJKHero HH;];eKca o6o3HaHaex, hxo sxo on;eHOHHa}i (estimated) BejiHHHHa.) 

TaKJKe Hcnojib3yexcH nojie onxHHecKoro noxoKa: {ui{tk)} (i=l ... n, k=l,2). 
P,Jis i'th xapaKxepnaH xoHKa MecxHocxn, Ui{ti) G M^ h Ui{t2) G M? npe^cxaBjiH- 
lox ee MecxonojiojKeHHe na nepBOM h BxopoM xa^pe cooxBexcxBeHHo. 

Hcnojib3yH Bt.imeynoMHHyxt.ie o6o3HaHeHHH, ti;ejii5 npe;];jio>KeHHoro ajiropnxMa 
cocxoHx B xoM, Hxo6t.i oii;eHHXb HcxHHHOH iio3y KaMept.1 H ee co6cxBeHHoe ^bh- 
jKeHHe: p{ti), R{ti), Ap{ti,t2) h AR{ti,t2) Hcnojib3yH nojie onxHnecKoro noxoKa 
{ui{tk)}, LI,KJI H iipH6jiH3HxejibHt.ie HanajibHbie ycjioBHJt: PE(ti), RE(ti), ApE(ti,t2) 

H Ai?s(tl,t2). 

0.8 HaBHraii,HOHHbm ajiropHTM. 

CjieflyiomHit pa3fleji onncbiBaex HaBHran;HOHHbiH ajiropnxM, Koxopbifi on;eHHBaex 
Bt.imeynoMHHyxt.ie napaMexpti. no3a n ^^BHJKenne sro KaMept.i nojiynenbi, ncnojiij- 
3yH IJ,KJI H nojie onxnnecKoro noxoKa ^jih ^Byx nocjie;];oBaxejibnbix Ka;i,poB. B 
oxjinnne ox Mexo;];a na3eMnbix opnenxnpoB nnKaKne xapaKxepnbie opnenxnpbi ne 
^ojiJKnbi 6bixb oGnapyjKenbi n pacno3nanbi. Tojibko cooxBexcxBne Mexc^^y fl^Byus 
nocjie;];oBaxejibnbiMH H3o6pa>KeHH}iMH ;];oji>Kno 6t.ixb naii^eno, nxo6bi nojiynnxb no- 
jie onxnnecKoro noxoKa. KaK 6bijio ynoMHnyxo b npeflbiflym;eM pa3^ejie, rpyGaa 
on,enKa hckombix napaMexpoB Hcnojib3yexcH KaK nepBoe npnGjiH^Kenne. O^naKo, 
xaK KaK ajiropnxM xojibko Hcnojib3yiox sxo KaK nanajibnoe npnGjinacenne n noBxop- 
no BbinncjiHex no3y n ^^BHJKenne nenocpe;];cxBenno, nnKaKoe cjioxcenne npe^bi^ym;HX 
omnGoK ne 6y;];ex HMexb Mecxo, n xonnocxb 6y;];ex coxpannxbCH. 

HoBbiii noflxo^ ocnoBan na cjieflyiom;eM naGjiio^ennn. TaK KaK IJ,KJI npe^ocxaB- 
jisex nn4)opMan;nio o cxpyKxype naGjiioflaeMoro Jian^macjixa, rjiy6nny naGjno^a- 
eMbix xapaKxepnbix xoneK Mecxnocxn onpe;];ejiHex no3a KaMepbi. Cjie;];oBaxejibno, 
ynnxbiBan no3y n ^Bnacenne KaMepbi, nojie onxnnecKoro noxoKa Moacex Gbixb o^no- 
3nanno onpe^ejieno. IJ,ejibio ajiropnxMa HBjiaexcH naxoacflenne no3bi h flBHacenna, 
Koxopbie npHBO^HX K nojiio onxnnecKoro noxoKa nanGojiee 6jin3KOMy, nacKOJiBKO 
3XO B03MO>Kno, K nafi^ennoMy Bbime nojiio onxnnecKoro noxoKa. 

E;];nnnnnbiH BeKxop n3 nojiH onxnnecKoro noxoKa 6y;];ex ncnojib30BaxbCH, nxo6bi 
onpe^ejiHXb orpannnennji na no3y KaMepbi n ee ^^Bnacenne. Hycxb ^G G M^ HBjiHex- 
CH MecxonojioJKenneM xapaKxepnon xohkh Mecxnocxn b xpexMepnoM npocxpancxBe. 
B ^Ba pa3Jinnnbie MOMenxa BpeMenn ti a t2, 3xa xapaKxepnaa xonKn Mecxnocxn 



npoeKTHpyexca na njiocKocxb H3o6paJKeHHH KaMept>i b tohkh u{ti) h u{t2). Hcnojib- 
ayii Mo^ejib ^bipoHHoit KaMepti, nojiynaeM u{ti),u{t2) G R^. HosBOjibTe ^q{ti) and 
^'q{t2) 6t.iTt> roMoreHHMMH npe^^cxaBjieHHHMH sthx MecTonojioacenHfl. Taxace mojk- 
Ho onHcaxB sth BCKTopa KaK BeKTopa h3 OHTHHecKoro n;eHTpoB KaMep k TOHKaM 
npoeKTHpoBaHHH Ha hjiockocth H3o6pa>KeHHH. HcnojibsyH HanajibHyio oij,eHKy nosbi 
KaMepbi B ti, jiHHHH, npoxoflamaH nepes PE{ti) h "^5(^1 ), Moacex 6t.iTB nepecenena c 
IJ,KJI. JI1060H ajiropHTM TpaccnpoBKH jiynefi Moxcex HcnojibaoBaxBca fl^jis SToii ii;ejiH. 
MecTonojiojKeHHe 3Toro nepeceneHHa oGosHaneHO KaK ^Ge- Chmboji "E^nnyKHero 
HH;];eKca npoacHHex tot (|)aKT, hto sTa TOHKa MecTHocTH - npe;];nojiaraeMoe mccto- 
nojiojKeHHe fljis xapaKTepnoii tohkh, KOToptiil Boo6iii;e 6yfleT OTjinnaTBCH ot hc- 
THHHoro MecTonojiojKeHHii xapaKTepHOH tohkh ^G. PasjiHHHe Mexc^^y hcthhhbim h 
OLi;eHeHHt.iM MecTonojioaceHHeM npoHcxoflHT hs ^Byx ochobhbix hctohhhkob: omnGKa 
B HanajiBHOM npe;];nojio>KeHHH j^jis nosbi h omnGKa b onpe;];ejieHHH ^Ge, BbisBaHHofl 
flHCKpeTH3an;HeH LI,KJI h ochobhbimh norpemnocTaMH. fl,Jis paayMHbix ouihGok na- 
HajiBHoro nojioaceHHH h LI,KJI - cBasannbix ouihGok, pfie tohkh ^Ge h ^G ^ojijkhbi 

GblTL. ^OCTaTOHHO 6jIH3KHM, HToGhl nOSBOJIHTB JIHHeapH3aiJ,HIO IJ,KJI BOKpyr ^Ge- 

06o3HaHa5i A^ nopMajit hjiockocth, KacaTejibHoit k DTM b TOHKe ^Ge, mojkho na- 
HHcaTb: 

N^^G - "^Ge) ^ (32) 

HcTHHHaa xapaKTepnaa TOHKa MecTHocTH ^G MoaceT Gbitb onHcana, Hcnojitaya 
HCTHHHbie napaMeTpti nosti: 

"^G = Rih) ■ ^q{t^) ■ A + p{t,) (33) 

S^ecB, A o6o3HaHaeT rjiyGnny xapaKTepnoH tohkh (to cctb paccTOHnne ot tohkh 
flp njiocKOCTH H3o6pa>KeHHH, cnpoeKTHpoBaHHoe na onTHHecKyio ocb). HoflpTaBjms 

dMD B (El: 

N^iX ■ R{h) ■ ^q{t,) + p{t,) - ^^Ge) = (34) 

Hs 3Toro BBipaJKeHHS rjiyGnna hcthhhoh xapaKTepnoii tohkh mojkct Gbitb pac- 
CHHTana, ncnojibsya on;eHonHoe nojioxceHne xapaKTepnofl tohkh: 

_ N^-^Ge - N^pjh) ,__, 

^ - NTRit,)Cqit,) ^^^^ 

IIoflCTaHOBKOH (^5ij o6paTHo B (15S]) nojiynaeM: 

-^r m, ^c u , f N^^GE-N^p{ti) \ ^ ,^ . ,_ 

HtoGbi ynpocTHTb oGosHaneHHH, R{ti) 6y^eT saMeneH Ri h anajiornHHO ^jih p{ti) 
H q{ti) i = 1,2. AR{ti,t2) H Ap{ti,t2) 6y;];yT saMeneHbi na Ru h P12 cooTBCTCTBeHHo. 
BepxHHit HH^eKc, onHCbiBaromHit cHCTeMy Koop^nnaT, b KOTopoii ^an BeKTop, 6y^eT 
TaKJKe onymen, sa HCKjnoneHHeM cjiynaeB, r^e TpeGyeTca oco6oe BHHMaHne k onncbi- 

BaeMBIM CHCTCMaM KOOp^HHaT. 06t.IHHO, Pi2 H g's HaXOflHTC3 B CHCTeMC KOOp^HHaT 

KaMept.1, B TO BpeMa KaK ocTajibnaa nacTb BCKTopoB ^ana b rjioGajiBHoit cHCTCMe 
KoopflHHaT. Hcnojibsya ynpomeHHbie oGosHaHeHHJi, (136|) moxcbt Gbitb nepenHcano 
KaK: 

R,q,N^ ^ Riq.N^ 




'P(R,q,,w)(p,-t3£) 



Phc. 4: FeoMeTpHHecKoe onncaHHe BbipaaceHHa f HU]) Hcnojibsya npoeKTHpyioiu;HH 
onepaTop 



^jiH Toro, HToGbi nojiyHHTt. 6ojiee npocToe BbipaaceHHe onpejiejinM cjie;];yioiri;HH 
npoeKTHpyiomHH onepaxop: 



V{u,s) 



I- 



us 



s^u 



(38) 



3tot onepaxop npoexTHpyex Bexxop na HopMajib no^npocTpancTBa k s, b^ojib 
HanpaBjieHHH u. Kax HjijiiocTpaij;HH, 3to npocTo iipoBepHTb, hto s^ ■ V{u, s)v = h 
V{u,s)u = 0. ^oGaBjiaa h BbiiHTaa Ge k (157|) . nocjie nepeynopa^oneHHa: 



G — Ge + 



I 



N^RiQi 



Pi 



I 



N^RiQi 



Gf 



HcnojiBsyH npoeKTHpyiomHil onepaxop, ( 139|) CTanoBHTca: 



(39) 



(40) 



y BBimeonncannoro BBipaacenna ecTB acnaa reoMeTpnnecKaa HHTepnpeTan;H3 
(cm. PncJlj). BeKTop ns Ge k pi npoeKTHpyexca na KacaxejiBnyio njiocKocxB. Hpo- 
eKTHpoBanne n^ex b^ojib nanpaBjienna RiQi, Koxopoe aBjiaexca nanpaBjienneM jiyna 
H3 onxnnecKoro n;enxpa KanepBi (pi ) , npoxo;],Hm;ero nepes cooxBexcxByion^yio xonxy 
H3o6pa>KenH3. 

Ham cjieflyiomHii mar 6y;];ex nepenoc G ns rjioGajiBnon cncxeMBi Koopflnnax - 
W B CHCxeMy Koopflnnax nepBon KanepBi Ci n saxeM k cncxeMe Koop^nnax Bxopoft 
KaMepBi 6*2. TaK xax pi and -Ri onncBiBaiox npeoGpasoBanne us Gi b W , mbi 6y;];eM 
HcnojiBSOBaxB o6paxnoe npeo6pa30Banne: 



^^G'=pi2+i?i2(i?r(G'-pi)) 

rioflcxanoBKa (HOj) b (HTj) ;];aex: 

^2G' = pi2 + i?i2/:(G'i,-pi) 

£ B nanncannoM BBime BBipaJKennn npe^cxaBjiaex: 



(41) 



(42) 



(43) 



N^RiQi 

Moacno nnxepnpHxnpoBaxB C KaK onepaxop o6paxnBiH k P: on npoexxnpyex 
BeKxopbi na Jiyn, npoflOJiacaiom;nH -Rigi b^ojib njiocKocxn, opxoronajiBnoft k A^. 



g2 - npoeKij,H3 hcthhhoh xapaKxepHofl tohkh mccthocth G. Tekhm oGpasoM, 
BeKTopbi g2 H ^^G ^ojiJKHL.1 coBHacTb. 3to Ha6jiK)^eHHe MOJKex GbiTb BbipajKeHo 
MaTeMaxHHecKH, npoexTHpya ^'^G na npo^ojixceHne jiyna q2'- 

02G = -Sl^. (p- . '^2g\ (44) 

m Km J 

B BbipaJKeHHH dB]), 5'2"/|5'2| ■ '^'^G HBJIHeXCH BejIHHHHOii '^2(7's npeKII,HH Ha q2- 

ripeoGpaaya dHj) h Hcnojitsya npoeKTHpyiomnfi onepaxop, mbi nojiynaeM: 



T 
I2 ■?2, 



■^^G = P(g2,g2)-^^G = (45) 



^^2(7 HBjiaexca npoeKi^Heit na opToroHajitHyio KOMnoneHT g2- Tax xax ^^(^ ^ gg 
;];oji>KHbi coBnaflaTb,3To npoexTHpoBaHHe flpjiyKHo ^aBaTb nyjieBofi BexTop. IIoflCTa- 
HOBKa (H2|) B ( H5|) npHBo^Hx K HameMy oKOHHaxejiBHOMy orpaHHHeHHio: 

P(g2,g2)[pi2 + i?i2/:(G's-pi)]=0 (46) 

3to orpaHHHeHHe BKjnoHaex no3Hn;Hio, opHeHTan;Hio h co6cTBHHoe ^BHaceHHe, 
onpe;i,ejiaeMoe na ochobc flByx Ka^poB KaMepti. Xoxa oho BKjiioHaiOT xpexMepHbie 
BCKTopa, acHo, HTo ero panr He MOJKex npeBbicHTb ^BOHKy Hs-aa Hcnojit>30BaHHa V, 
KOTopbiii npoeKTHpyex M^ na ^ByMepnoe no^^npocxpaHCXBO. 

TaKoe orpaHHHeHHe mojkct Gbitb ycxaHOBjieHo ^jih Ka>K;];oro BexTopa b nojie 
onxHHecKoro noxoKa, noKa He 6jfl,eT nojiynena HecHHryjiapnaa cHcxeMa. Tax xax 
^BeHa^ij,axb napaMexpoB ^ojijkhbi 6t.ixB on;eHeHL.i (mecxb fljia noati h mecxb ^jia 
coGcxBeHHoro ^BHaceHHa), no KpaflHefl Mepe, mecxb BeKxopoB onxHHecKoro noxoKa 
xpeGyioxca ^jia pemeHHa cncxeMbi. Ho 3xo - npaBHjiBHoe saKjiioHeHHe ^jia HejiHHeH- 
HOH npoGjieMbi. Ecjih mbi HcnojibsyeM Mexo^i; Hxepan;HH Faycca-HbioxoHa, xo fl,ejia- 
eu jiHHeapH3ai];HK) Hamefi npo6jieML.i okojio npnGjiHaceHHoro pemeHHH. Hafl^eHHaH 
MaxpHij,a 6y^ex Bcer^a CHHryjiapHofl ^jih mecxH xoneK (c nyjieBtiM ^exepMHHan- 
xom), KaK HHCjieHHoe Mo^^ejinpoBanne ^^eMOHcxpnpyex. TaKHM oGpasoM, Heo6xofl;ii- 
Mo HcnojibsoBaxB, no Kpanneit Mepe, ceMt xoneK, nxo6t.i nojiynnxt necHHryjispnoe 
jinnennoe npnGjinacenne. OGbinno, neM Gojibme BeKxopoB 6y;i,ex Hcnojit.30BaxL.c3, 
Hxo6t.i onpeflejiHXB nepeonpe^ejiennyio cncxeny, xcm 6ojiee ycxonnnBO pemenne. 
BnHMaHHe ^ojixcno Gbixb npnBjieneno k (|)aKxy, nxo 6t.ijio nojiyneno nejinnennoe 
orpaHHHeHHe. TaKHM o6pa30M, Hxepan;HOHHa5i cxena 6yflex ncnojiBaoBaxbca, nxo- 
6t.i pemnxb sxy cncxeMy. YcxoHnHBtiH ajiropnxM, Koxopbiit ncnojibsyex HxepaH;HH 
Faycca-Hbioxona n M-on;eHn];HK onncan b [9j. Mti nannnacM ncnojibsoBaxb mcxo^ 
Levenberg-Marquardt, ecjin mcxo^i; Faycca-HtioxoHa nocjie necKOJiBKHx Hxepan;HH 
npeKpaxHji cxo^nxbca. 3xh ^Ba ajiropnxMa peajiHsoBanbi b (|)yHKn;HH lsqnonlin() 
naxexa Matlab. IIpHMeHHMocxb, xonnocxb n Ha^eacHocxb ajiropnxMa Gbijih npoBe- 
peHBi nepea nncjiennoe Mo^ejinpoBanne n jia6opaxopnL.ie SKcnepHMenxti. 

Bojiee y^oGno ncnojibsoBaxt 6ojiee ycxonnnBoe fl^jiii Hxepan,HH pemenne, skbh- 
BajienxHoe ypaBnennio 



V{q2,q2) [Pi2 + R12C, (Ge^-p^)] /r^GI = (47) 

HcnojiL.30BaHHe sxoii HopMajiHsoBaHHoii 4)opMbi ypaBHenHit npe^oxBpamaex no- 
jiynenne ncBepnoro xpnBnajibnoro pemenna, Kor;];a fl,Be no3nn;nn naxo;];3xca b e^n- 
noH xonxe na Mecxnocxn. 



0.9 HaBHraLi,HOHHbm ajiropHTM, ocHOBaHHbift na KOMnbio- 

TepHOM SpeHHH, JlJl^ KOppeKLI,HH HHepLI,HajIbHOH HaBH- 

raii,HH c noMombK) (J^HjibTpa KajiMana. 

OcHOBaHHbie Ha KOMnbioTepHOM speHHH HaBHrai];HOHHi>ie ajiropHTMbi Gmjih rjiaB- 
HOH Hccjie^oBaTejibCKOH npoGjieMoii b TeneHne npomjibix ^ecaTHjieTHfi. AjiropHTM, 
HcnojibsyeMBiil b 3toh cxaxbe, ocHOBan na reoMexpHH MHornx H3o6pa>KeHHil h Kap- 
xe MecxHocxH. C noMombio sxoro ueTojia mbi nojiynaeM no3Hij,Hio h opHeHxaij,Hio 
HaGjiioflaiomeH KaMept>i. C ^pyrofi cxopoHbi mbi nojiynaeM xe ace caMbie ^aHHbie h3 
HHepii;HajibHt.ix HaBHraii;HOHHt>ix Mexo;];oB. Hxo6t.i cKoppeKxnpoBaxb 3xh ;];Ba pesyjit.- 
xaxa, HcnojiBsyexcH cJjHjibxp KajiMana. Mti HcnojitayeM b sxoii cxaxbe pacmnpeH- 
Hbiit 4)Hjibxp KajiMana fl,jis HejiHHeitHbix ypaBHeHHfi [12j. 

/],jia HHepn;HajiBHL.ix HaBHran;HOHHL.ix BbiHHCJieHHH HcnojibsoBajica Hnepi^HajiB- 
HL.IH HaBHrai];HOHHt.iH CHcxeMHBifl HaKex j^jis Matlab |13] . 

Bxofl 4)HJiBxpa KajiMana cocxohx h3 ^Byx nacxeii. riepBtifi - nepeMennbie X ^jia 
ypaBHeHHH ;i,Bii>KeHH5i. B HameM cjiynae sxo - HHepii,iiajibHL.ie HaBHran;HOHHt.ie ypaB- 
HeHHH. BeKxop X cocxohx h3 n5ixHa^n;axH KOMnonenxoB: [6x6y6z6VxSVy6VzS(f)666ipaxayazbxbybz 
KoopflHHaxBi Sx6y6z onpe^ejienti Kax pasHima Mcac^y peajiBHoii no3Hn;HeH KaMcpt-i 
H no3Hii,HeH, nojiyHenHbift us HHepii,HajibHoro HaBHraii;HOHHoro BbiHHCjieHHH. Ilepe- 
MeHHbie 6Vx SVy 6Vz onpe^ejienbi KaK pa3HHii,a MejK^y peajitHoii cKopocxbio KaMcpti 
H CKopocxbio, iiojiyHeHHoii h3 HHepii;HajibHoro HaBHraii,iioHHoro BbiHHCjieHHa. He- 
peMCHHaa 6(j) S6 6ip onpe;i,ejieHa KaK yrjiti Sfijiepa Maxpiiii,L.i Dr * -Dj. S^ect Dr 
Maxpima onpe^ejiena peajibHbiMH yrjiaMH Bfijiepa KaMcpbi oxHocHxejibHo jioKaxib- 
HOH cHcxeMbi KoopflHHax (L- frame). C ^pyrofl cxopoHbi Dc Maxpiin;a onpe^ejiena 
yrjiaMH Bfijiepa KaMcpbi oxHocHxejibHo jioKajibHoii chcxcmbi Koop;i,iiHax (L-frame), 
nojiyHeHHbiMii 113 HHepn;HajibHoro HaBHran;iioHHoro BbiHiicjicHHa. Heo6xo^iiMO o6pa- 
XHXB BHHMaHHe, Hxo HafifleHKBie yrjibi Sfijiepa 6(j) 66 Sip HE sKBUBajieHXHbi pa3HHii,e 
Meatfly peajibHbiMH yrjraMH Bttnepa h yrjiaMH 3Hjiepa, nojiyHenHbiMH h3 HHepii;H- 
ajiBHoro HaBHraii;HOHHoro BbiHHcxreHHa. OflnaKo fljia MajieHbKiix 3HaHeHHH 6(j) S6 Sip 
nonpBKH K 3XHM yxjiaM Moryx Gbixt. flo6aBjieHt.i jiHHeiiHo h xaKiiM o6pa30M sxh yrjibi 
Moryx HcnojiB30Baxbca b cJ)HJibxpe KajiMana b cjiynae MajieHbKnx ouihGok. TaKofl 
Bt.i6op yrjioB cflCJian, nocKOJitKy 4)opMyjit.i, onncbiBaiomHe iix 3BOjiion;Hio, naMHoro 
Gojiee npocxbi, hcm (|)opMyjit.i, onHCbiBaioiu;He 3BOJiion;Hio pa3HHn;t.i MCJKfly yrjiaMH 
3Hjiepa. riepeMeHKbie a^ ay a^ onpeflejienbi BCKxopoM cMemenns ycKopeHHa b HHep- 
Li;HajibHL.ix HaBHran;HOHHt.ix ii3MepeHH5ix. nepeMeHHbie h^hyhz onpeflejienbi BCKxopoM 
rupocKonnHecKoro CMcmeniia b HHepii,HajiBHt.ix HaBHran;HOHHL.ix H3MepeHH3x. 

Bxopoii Bxofl (|)iijiBxpa KajiMana - Z-pe3yjibxax ii3MepeHiiH, ocHOBannbix na na- 
BHraii;HOHHbix ajiropnxMax KOMnbioxepHoro 3peHHa. Bexxop Z cocxohx h3 mecxH 

KOMnOHCHXOB [5Xm St/m SZm S(j)m S9m Slpm]- KoOpflHHaXbl SXm Sym SZm 5IBJI5IK)XC5I 

pa3HHn;eH MCJKfly no3Hn;HeH KaMept.1, H3MepeHHOH na ochobc HaBHran;HOHHoro aji- 
ropiixMa KOMntioxepHoro 3peHii5i, h no3Hn;HeH, nojiyHeHHtifl h3 HHepn;HajibHoro na- 
BHran;HOHHoro Bt.iHHCJieHH3. HepeMeHHaa 66m, 61pm onpeflexrena KaK yrjibi Bfijiepa 
MaxpHn;B.i Dm * -Dj- Sflecb Dm MaxpHi];a onpefleireHa yrjiaMH Biijrepa KaMepti oxho- 
CHxejiBHo oxHocHxejiBHo jioKajiBHOH cHcxcMBi KoopflHHax (L-frame), H3MepeHHt.IX Ha 
ocHOBe HaBHraH;HOHHoro ajiropnxMa KOMnbioxepnoro 3peHH5i. C flpyrofl cxopoHbi Dc 
MaxpHii,a onpeflcjiena yrjiaMH Sfijiepa KaMcpti oxHocuxejibHo jioKajiBHofi chcxcmbi 
KoopflHHax (L-frame), nojiyneHHtiMH 113 HHepn;HajiBHoro HaBHraii,HOHHoe BbiHHCJie- 
HHii. Hycxb nepeMeHHaa k onpeflcjiaex hhcjio maroB fljia flHCKpexH3aii;HH BpeMenn, 
HcnojiB3yeMt.ix b (|)HjiL.xpe KajiMana. 

Mbi nojiaracM, hxo oiiihGkii Meac^y 3HaHeHH3MH, njiyneHnbiMH 113 iiHepii,HajiB- 



Horo HaBHraii;HOHHoro BbiHHCJieHHa, h peajibHtiMH sHaneHKHMH jiHHefiHo saBHcaT ot 
inyMa. CooTBexcTByiomaa KOBapnai^HOHHaa MaTpHn;a inyMa oGosHaneHa Qk- /I,Ha- 
roHajiBHbie sjieMCHTbi Qk cooTBexcTByioT cKopocxH, onpe;];eji3ioTC3 myMOM ycKope- 
HHS H nponopn,HOHajibHL.i dt^: Qv ^ dt^-, r^e dt HHxepBaji BpeMCHH Meac^y tk h 
tk-i- dt = tk — tk-i- /I,HaroHajibHL.ie sjieMeHTbi Qk cooTBexcTByioT yrjiaM Sfljiepa, 
onpe^ejisiioTca rnpocKonHHecKHM inyMOM h nponopn;HOHajiBHL.i dt: Qa ~ dt. 

Mbi npe;];nojiaraeM, hto xaxace omnGKH Meac^^y sHaneHHiiMH, nojiyHeHHbiMH na 
ocHOBe HaBHran;HOHHoro ajiropHXMa KOMnbioTepHoro speHHH, h peajitHbiMH sHane- 
HHSMH jiHHefiHo saBHcax OT uiyMa. CooTBexcTByiomaa KOBapHan;HOHHaH MaTpHn;a 
myMa oGosHaneHa i?^. AnajiHs oiuhGok, ^aiomnfi sTy MaTpHn,y, onHcan b |14| . 

YpaBHeHHsi 4)HjibTpa KajiMana onncbiBaioT xax 3bojiioij,hio oij,eHOK coctohhh3 
Xk, onHcaHHbix Bt.ime,TaK h 3BOJiion;Hio KOBapHaii;HOHHOH MaxpHi^bi Pk flJia nepe- 

MeHHblX Xk- 

HtoGbi HanHcaxB ypaBHeHHa (|)HjibTpa KajiMana, mbi floJiacHbi onpe^ejiHXB eme 
^Be 15x15 MaTpHn;L.i: Hk h Ak- MaTpHn;a Hk SBjiaexcs aKoGnaHOM HSMepeHHa, onn- 
cbiBaiomaH cBHSb Me>K;];y npe;i,cKa3aHHbiM HSMepeHHeM if ^ * Xk h (|)aKTHHecKHM hs- 
MepeHHeM Zk, onpe^ejieHHtiM Btime. /jHaroHajibHbie sjieMeHTti iffc(l,l), Hk{2,2), 
Hk{3,3), onHCbiBaiomHe Koop^HHaTy h sjieMeHTbi Hk{4:,7), Hk{5,8), Hk{Q,9), onn- 
cbiBaiomHe yrjiBi, paBHbi eflHHHn,e. OcxajibHaa nacxb sjieMenxoB paBHa nyjiio. 

Ak MaTpHii,a 5Iko6h, onHCbiBaromaa sbojuoi^hio BCKTopa Xk. Tohhoc BtipajKeHHe 
^ji3 3TOH MaTpHii;i>i sBjisexcii oHeHb cjiojKHBiM, HosTOMy ML.I HcnojiBsyeM npnGjiH- 
jKCHHyio 4)opMyjiy fl^jis Ak, npeneGperaa 3(|)(|)eKTaMH KopnojiHca, BpamenneM 3eM- 
jiH H Tax ^ajiee. IIoaBojiBTe (p 6 ip Gbitb yrjiaMH 3Hjiepa b L-franie, dV aBjiaexcH 
BCKTopoM deltaV, nojiyHeHHbiM h3 HHepn;HajibHt.ix HaBHran;HOHHt.ix HSMepeHHfi, f^^c 
BCKTop ycKopeHHa B L-frame, DCM^_^^y ~ MaTpHn;a HanpaBjisiomnx KocHHycoB 
(fljis nepeBo;];a h3 KoopflHHaTHoit cncTeMbi jieTaxejibHoro annapaxa b L-frame). 

OopMyjiBi onpe^ejiHiomHe Ak, cjie/i;yK)iri;He: 

cosi^ip) sm('?/') 
^DCAf = I - sm(V^) cos(^) I (48) 

1 

cos(^) -sin(^) 
Qdcm = I 1 I (49) 

sin(^) cos(^) 

1 

^DCM = I cos(0) sm(0) I (50) 
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cos(0) 
— sin(0) 


sin(0) 
cos(0) 



DCM^_^^_l = ^dcmQdcm'^dcm (51) 



Phi{l :3,4: 6) = 1 (53) 




Phi{A : 6, 7 : 9) = I /,ec(3) -/.ec(l) | (54) 

Jvec\^) Jvec\^) U 



Phi{7 : 9, 10 : 12) = -DCM^_^^i (55) 

Phi{4: : 6, 13 : 15) = -DCM^_^^_^ (56) 

OcTajibHbie sjieMeHTbi MaTpHn;t.i Phi paBHbi nyjiio. 

Ak = I + Phi dt (57) 

ypaBHeHHH (|)Hjii>Tpa KajiMana fl^jis BpeMeHHofl sbojimi^hh cjieflyiomHe 

X^ = [0 00000000 a^k-1 Sfc-i ^zk-i hk-i Kk-i ^zk-i] (58) 

P,7 = A^Pk-iAl + Q,_i (59) 

ypaBHeHHH cJ)HjiBTpa KajiMana npoeKXHpyioT cocxoHHHe h KOBapHan;HOHHyio 
MaTpHn;y c npe;];t.i;];yiii;ero BpeMeHHoro mara A; — 1 na xeKymnfl BpeMeHHofi mar k. 
YpaBHeHHa 4)Hjit.Tpa KajiMana ^Jia HSMepeHna cjie^yiomHe: 

K, = P^Hl{H,P^Hl + R,)-^ (60) 

Xfc = X- + Ku{Z,, - HkX^) (61) 

Pfc = (/ - KkHk)P,{I - KkHkf + KkRkKl (62) 

YpaBHeHHa (|)HjibTpa KajiMana j^Jia HSMepeHna HcnpaBjiJiioT cocTOHnne ii KOBa- 

pHaLtHOHHOH MaTpimy B COOTBeTCTBHH c IISMepeHHeM Zk- 

Y{a&j\eBHhm Bexxop Xk ncnojibsyexcH, htoGbi oGhobhtb Koop;i,iiHaTt.i, cKopocTn, 
yrjibi 3Hjiepa, CMemeniie ycKopeHH h rnpocKonHHecKoe CMemeHiie ^jih HHepii;Haj[i>- 
HL.IX HaBHran;HOHHt.ix BbiHucjienHfl na cjie^^yiomeM mare. 

HiicjieHHtie pacnexbi 6l.ijiii peajiiisoBaHHti, hto6l.i Hccjie^oBaxb 3cJ)cJ)eKTHBHOCTB 
(|)HjibTpa KajiMana h htoGbi o6te;];HHHTt> sth jxea. HaBHraLi;HOHHt.ix ajiropHXMa. Ha 
Phc. Ombi MOJKeM Bii^exB, HTO ^jiH OTKoppeKTHpoBaHHoro nyxH omnGxa Koop^iina- 
TBi, nojiyHeHHbiH na ocHOBe ^Byx HaBHraii;HOHHbix Mexo^^oB c (|)HjibTpaLi;HeH KajiMa- 
na, HaMHoro MCHtme neM HHepn;HajibHa5i HaBHran;HOHHa5i omnGKa Koop^iiHaxBi, no- 
jiyneHHaa 6e3 (|)HjibTpa KajibMana. yjiynmeHHbie pesyjibTaxbi c noMombio (Jjujibxpa 
KajiMana 6bijih nojiyneHbi xaKJKe fljrsi CKopocxH, necMoxpa na xo, hxo 3xa CKopocxt 
He HSMepaexca nanpaMyio HaBiiraii,HOHHbiM axrropHXMOM, Hcnojib3yiom;HM KOMnbio- 
xepHoe speHHe Phc. El 

0.10 BblBO^^bl 

AjiropHXM j\jiii oi];eHKH hosbi h ;];BH>KeHH5i, HcnojiB3yiom;HH cooxBexcxByiom;He xohkh 
B H3o6pa>KeHHax h IJ,KJI 6biji npe^cxaBjien c HcnojibaoBaHHeM (|)Hjibxpa KajiMana. 
LI^KJI cjiyjKHx KaK rjioGajiBHaa cnpaBOHnaa HH(|)opMan;H3, h ee j\a.BHiAe ncnojibsyiox- 
C3 fljia xoro, hxo6bi BoccxaHOBHXb a6cojiioxHyio hoshi^hio h opHeHxan,Hio KaMepti. 
B HHCJieHHbix pacnexax oi];eHKa j\Jiii hoshi^hh h CKopocxH Haxo;];axcH ;];ocxaxoHHO 
xoHHO, HxoGbi npe;];oxBpaxHXb naKonjieHHtie omnGKH h npe;],oxBpaxHXb ;],peH(J3 xpa- 
eKxopHH. 



BjiaroflapHOCTb 

Mbi 6t>i xoxejiH no6jiaro;];apHTb Ponena Jlepnepa, 3xy^a PMBjinna h XeKTopa Pot- 
niTeflHa aa oneHb nojiesHbie KOHcyjibTan,HH 




(b) 
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Phc. 5: OmnGKH no3Hii;HH ((a) flJiH KoopflHHaTbi x (b) ^jis KOop^HHaxbi y (c) fljis 
Koop;];HHaTL.i z). OmnGKH HHepn;HajibHoro ^^peflc^Da oxMeHeHbi KpacHofi jiHHHefl, h 
ouihGkh, HcnpaBjieHHbie BH;];eo-HaBHraiJ,HeH, OTMeneHbi CHHefl jiHHHefl. 



delta-velocity in m/s 




(b) 

Phc. 6: (a) oiiihGkh CKopocTH ^jih HHepi];HajibHoro flpeH(|)a (x y z KOMnoHeHTbi) , h 
(b) omnGKH cKopocxH, HcnpaBjieHHbie BHfleo-HaBHraii,Heii (x y z KOMnoneHTbi) . 
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